#include <p32xxxx.h>

#include "types.h"
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "FreeRTOSConfig.h"
#include "main_peripherals.h"
#include "driverRN41_UART_DMA_ISR_TASK.h"
#include "rtos_queues.h"
#include "commands.h"

void rnSend(int length, BYTE * buffer);

#define PACKET_START  '@'
#define PACKET_END '^'
#define PACKET_CONFIRM_REQUEST '!'
#define PACKET_CONFIRMED '%'
#define PACKET_STATUS '#'

#define CIRCULAR_BUFFER_POSITION(exp, m) (exp) & (m - 1)

// receive from DMA
#define RN_RECEIVE_BUFFER 4096

struct {
    BYTE buffer[RN_RECEIVE_BUFFER];
    SHORT unparsed;
    BYTE * pos;
} receive;

#define RECEIVE_END_POS (receive.buffer + RN_RECEIVE_BUFFER)

#define RECEIVE_WRAP        { if (receive.pos == RECEIVE_END_POS) receive.pos = receive.buffer; }
#define RECEIVE_ADVANCE     { receive.unparsed--; receive.pos++; RECEIVE_WRAP; }

BYTE        rnSendId = 0;
volatile BYTE        packetsToConfirm = 0;
BYTE        autoRelease = 0;

TickType_t  lastReceivedData;

BYTE rnTxBuffer[256];

BYTE * rnGetTxBuffer()
{
    return rnTxBuffer;
}


void rnStartReceive()
{
	//DmaChnOpen(DMA_BT_RX, DMA_CHN_PRI1, DMA_OPEN_DEFAULT);
	//DmaChnSetEventControl(DMA_BT_RX, DMA_EV_START_IRQ_EN|DMA_EV_START_IRQ(DMA_BT_RX_ISR));
	//DmaChnSetTxfer(DMA_WIFI_RX, (void*)&uartReg[UART_WIFI]->rx.reg, wifiInputRotating, 1, 512, 1);

    //DCH3ECONbits.CHSIRQ = _UART2_RX_IRQ;
    DCH3ECON = _DCH3ECON_SIRQEN_MASK |
                (_UART2_RX_IRQ << _DCH3ECON_CHSIRQ_POSITION);

    DCH3INT = _DCH3INT_CHBCIE_MASK;

    DCH3SSA = KVA_TO_PA(&U2RXREG);
    DCH3SSIZ = 1;
    DCH3SPTR = 0;

    DCH3DSA = KVA_TO_PA(receive.buffer);
    DCH3DSIZ = RN_RECEIVE_BUFFER;
    DCH3DPTR = 0;

    DCH3CSIZ = 1;
    DCH3CPTR = 0;

    DCH3CON = (1 << _DCH3CON_CHPRI_POSITION) |
                _DCH3CON_CHAEN_MASK | // NEEDED?
                _DCH3CON_CHEN_MASK;

    //DmaChnSetTxfer(DMA_BT_RX, (void*)&uartReg[UART_BT]->rx.reg, receive.buffer, 1, RN_RECEIVE_BUFFER, 1);
	//DmaChnSetEvEnableFlags(DMA_BT_RX, DMA_EV_BLOCK_DONE);
	//DmaChnEnable(DMA_BT_RX);

    IPC9bits.DMA3IP = 5;

    IEC1SET = _IEC1_DMA3IE_MASK;

	//INTSetVectorPriority(INT_VECTOR_DMA(DMA_BT_RX), INT_PRIORITY_LEVEL_5);
	//INTSetVectorSubPriority(INT_VECTOR_DMA(DMA_BT_RX), INT_SUB_PRIORITY_LEVEL_0);
	//INTEnable(INT_SOURCE_DMA(DMA_BT_RX), INT_ENABLED);
}

unsigned char rnGetParam(int pos)
{
    pos = CIRCULAR_BUFFER_POSITION(pos, RN_RECEIVE_BUFFER);
    return receive.buffer[pos];
}

BOOL rnTransmitAndConfirmBlock(BYTE * buffer, unsigned short length)
{
    BYTE timeout = 0;
    extern SemaphoreHandle_t semRnTxDMA, semRnTxConfirm;

    while (++timeout < 10)
    {
        rnSend(length, buffer);
        if (!xSemaphoreTake(semRnTxDMA, configTICK_RATE_HZ * 100)) return FALSE;

        while (U2STAbits.UTXBF);
//        while (!UARTTransmitterIsReady(UART_BT));
        U2TXREG = PACKET_CONFIRM_REQUEST;

//        UARTSendDataByte(UART_BT, PACKET_CONFIRM_REQUEST);

        if (xSemaphoreTake(semRnTxConfirm, configTICK_RATE_HZ / 2)) break;
    }

    if (timeout < 10) rnSendId++;
    return timeout < 10;
}

void rnTransmitAndRelease(BYTE * buffer, unsigned short length)
{
    autoRelease = 1;
    rnSend(length, buffer);
}

void rnReleaseTx()
{
    extern SemaphoreHandle_t semRnTxBus;
    xSemaphoreGive(semRnTxBus);
}

BOOL rnClaimTx(int timeout)
{
    extern SemaphoreHandle_t semRnTxBus;
    if (xSemaphoreTake(semRnTxBus,timeout))
    {
        rnSendId++;
        return TRUE;
    }
    return FALSE;
}

void rnSend(int length, BYTE * buffer)
{
    while (packetsToConfirm > 0)
    {
        if (U2STAbits.UTXBF == 0)
        {
            U2TXREG = PACKET_CONFIRMED;
            packetsToConfirm--;
        }
//        if (UARTTransmitterIsReady(UART_BT))
//        {
//            UARTSendDataByte(UART_BT, PACKET_CONFIRMED);
//            packetsToConfirm--;
//        }
    }
    while (U2STAbits.UTXBF);
    U2TXREG = PACKET_START;
    //while (!UARTTransmitterIsReady(UART_BT));
    //UARTSendDataByte(UART_BT, PACKET_START);
    while (U2STAbits.UTXBF);
    U2TXREG = rnSendId;
    //while (!UARTTransmitterIsReady(UART_BT));
    //UARTSendDataByte(UART_BT, rnSendId);
    while (U2STAbits.UTXBF);
    U2TXREG = length & 0xFF;
    //while (!UARTTransmitterIsReady(UART_BT));
    //UARTSendDataByte(UART_BT, length & 0xFF);
    while (U2STAbits.UTXBF);
    U2TXREG = length >> 8;
    //while (!UARTTransmitterIsReady(UART_BT));
    //UARTSendDataByte(UART_BT, length >> 8);
    IFS1CLR = _IFS1_U2TXIF_MASK;

    DCH2INTCLR = _DCH2INT_CHBCIF_MASK;

    DCH2ECON = _DCH2ECON_SIRQEN_MASK |
                (_UART2_TX_IRQ << _DCH2ECON_CHSIRQ_POSITION);


	//DmaChnOpen(DMA_BT_TX, DMA_CHN_PRI2, DMA_OPEN_DEFAULT);

	//DmaChnSetEventControl(
	//		DMA_BT_TX,
	//		DMA_EV_START_IRQ_EN |
	//		DMA_EV_START_IRQ(DMA_BT_TX_ISR));

    DCH2SSA = KVA_TO_PA(buffer);
    DCH2SSIZ = length;
    DCH2SPTR = 0;

    DCH2DSA = KVA_TO_PA(&U2TXREG);
    DCH2DSIZ = 1;
    DCH2DPTR = 0;

    DCH2CSIZ = 1;
    DCH2CPTR = 0;
	//DmaChnSetTxfer(DMA_BT_TX, buffer, (void*)&uartReg[UART_BT]->tx.reg, length, 1, 1);

    DCH2INTSET = _DCH2INT_CHBCIE_MASK;

    DCH2CONSET = /*_DCH2CON_CHAEN_MASK |*/ _DCH2CON_CHEN_MASK |
                (2 << _DCH2CON_CHPRI_POSITION);

    //	DmaChnSetEvEnableFlags(DMA_BT_TX, DMA_EV_BLOCK_DONE);
    //DCH2ECONSET = _DCH2ECON_CFORCE_MASK;

	/*if (DmaChnStartTxfer(DMA_BT_TX, DMA_WAIT_NOT, 0) != DMA_TXFER_OK)
	{
        while(1)
        {
            portNOP();
        }
		return;
	}*/
//	INTSetVectorPriority(INT_VECTOR_DMA(DMA_BT_TX), INT_PRIORITY_LEVEL_4);
//	INTSetVectorSubPriority(INT_VECTOR_DMA(DMA_BT_TX), INT_SUB_PRIORITY_LEVEL_0);

    IPC9bits.DMA2IP = 4;
//	INTEnable(INT_SOURCE_DMA(DMA_BT_TX), INT_ENABLED);

    IEC1SET = _IEC1_DMA2IE_MASK;
}



int oldPointer = 0;
void rnReceiveFromDMA()
{
	U2STACLR = _U2STA_OERR_MASK;

	if (DCH3CONbits.CHEN == 0)
	{
		rnStartReceive();
	}
	int pointer;
	pointer = DCH3DPTR; //DmaChnGetDstPnt(DMA_BT_RX);
	if (pointer == oldPointer) return;

	int p = pointer - oldPointer;
	if (p < 0) p += RN_RECEIVE_BUFFER;
	receive.unparsed += p;

	oldPointer = pointer;
}

void rnParsePacket(int pos, int length)
{
	int i;

    BYTE cmdId = receive.buffer[pos++];
    pos = CIRCULAR_BUFFER_POSITION(pos, RN_RECEIVE_BUFFER);

    struct CommandIn_t cmd;
    cmd.command = cmdId;

    cmd.paramLength = length - 1;
    cmd.paramPosition = pos;

    commandPost(&cmd);
}

void rnParseInput()
{
	// needs minimum of 4 char: START, PACKET TYPE, LENGTH, END
	while (receive.unparsed > 0)
	{
        if (*receive.pos == PACKET_CONFIRM_REQUEST)
        {
            if (rnClaimTx(0))
            {
                if (U2STAbits.UTXBF == 0) U2TXREG = PACKET_CONFIRMED;
                //if (UARTTransmitterIsReady(UART_BT)) UARTSendDataByte(UART_BT, PACKET_CONFIRMED);
                else packetsToConfirm++;
                rnReleaseTx();
            }
            else
            {
                packetsToConfirm++;
            }

            RECEIVE_ADVANCE;
        }
        else if (*receive.pos == PACKET_CONFIRMED)
        {
            extern SemaphoreHandle_t semRnTxConfirm;
            xSemaphoreGive(semRnTxConfirm);
            RECEIVE_ADVANCE;
            continue;
        }
        else if (*receive.pos == PACKET_STATUS)
        {
            if (receive.unparsed < 7) break;

            BYTE connect = 1;
            RECEIVE_ADVANCE;
            if (*receive.pos == 'D')
            {
                // scan for disconnect msg
                const char msg[10] = {'D','I','S','C','O','N','N','E','C','T'};
                BYTE pos;
                for (pos = 1; pos < 10; pos++)
                {
                    RECEIVE_ADVANCE;
                    if (*receive.pos != msg[pos])
                    {
                        // breakbreakbreak!
                        break;
                    }
                }
                if (pos != 10) continue;
                // DISCONNECTION!
            }
            else if (*receive.pos == 'C')
            {
                // scan for disconnect msg
                const char msg[7] = {'C','O','N','N','E','C','T'};
                BYTE pos;
                for (pos = 1; pos < 7; pos++)
                {
                    RECEIVE_ADVANCE;
                    if (*receive.pos != msg[pos])
                    {
                        break;
                    }
                }
                if (pos != 7) continue;
                // CONNECTION !
            }
            else if (*receive.pos == 'R')
            {
                // scan for disconnect msg
                const char msg[7] = {'R','E','B','O','O','T'};
                BYTE pos;
                for (pos = 1; pos < 6; pos++)
                {
                    RECEIVE_ADVANCE;
                    if (*receive.pos != msg[pos])
                    {
                        break;
                    }
                }
                if (pos != 6) continue;
                // REBOOT !
                portNOP();
            }
        }
        else if (*receive.pos == PACKET_START)
        {
            if (receive.unparsed < 5) break;

            BYTE * b = receive.pos + 2;
            if (b >= RECEIVE_END_POS) b -= RN_RECEIVE_BUFFER;
            SHORT len = (*b);// | ((*lenLSB) << 8);
            b++;
            if (b >= RECEIVE_END_POS) b -= RN_RECEIVE_BUFFER;
            len |= (*b) << 8;

            if (receive.unparsed < 5 + len) break;

            b += len + 1;
            if (b >= RECEIVE_END_POS) b -= RN_RECEIVE_BUFFER;
            if (*b != PACKET_END)
            {
                // invalid packet end;
                RECEIVE_ADVANCE;
                continue;
            }

            RECEIVE_ADVANCE;
            BYTE packetId = *receive.pos;
            RECEIVE_ADVANCE; // len MSB
            RECEIVE_ADVANCE; // len LSB

            rnParsePacket(receive.pos - receive.buffer + 1, len);
            receive.pos += len + 2;
            if (receive.pos >= RECEIVE_END_POS) receive.pos -= RN_RECEIVE_BUFFER;
            receive.unparsed -= 2 + len;
        }
        else
        {
            RECEIVE_ADVANCE;
        }

	}
}

void TaskRN41(void * parameters)
{
	int i,c;
	TickType_t taskSleep;

	taskSleep = xTaskGetTickCount();
    receive.pos = receive.buffer;

    extern SemaphoreHandle_t semRnTxBus;
    xSemaphoreGive(semRnTxBus);
    
	for(;;)
	{
		vTaskDelayUntil(&taskSleep,1);

        if (U2STAbits.FERR == 1)
        {
            U2STAbits.FERR = 0;
        }
		/*if (lastReceivedData + configTICK_RATE_HZ * 20 < taskSleep)
		{
			BLUETOOTH_RESET_ON();
			vTaskDelay(configTICK_RATE_HZ / 10);
			BLUETOOTH_RESET_OFF();
			lastReceivedData = taskSleep - configTICK_RATE_HZ;
		}*/

		// parse input
		rnReceiveFromDMA();
		rnParseInput();

        if (packetsToConfirm > 0)
        {
            if (rnClaimTx(1))
            {
                while (packetsToConfirm > 0 && U2STAbits.UTXBF == 0) //UARTTransmitterIsReady(UART_BT))
                {
                    packetsToConfirm--;
                    //UARTSendDataByte(UART_BT, PACKET_CONFIRMED);
                    U2TXREG = PACKET_CONFIRMED;
                }
                rnReleaseTx();
            }
        }
	}
}


void __attribute__((vector(DMA_BT_RX_VECTOR), interrupt(ipl5))) ISR_BT_DMA_RX()
{
	// event flags when getting the interrupt
//	INTClearFlag(INT_SOURCE_DMA(DMA_BT_RX));
    IFS1CLR = _IFS1_DMA3IF_MASK;
//	static int evFlags;
//	evFlags = DmaChnGetEvFlags(DMA_BT_RX);	// get the event flags

	// restart!
//    if (evFlags & DMA_EV_BLOCK_DONE)
    if (DCH3INTbits.CHBCIF)
    {
//		DmaChnClrEvFlags(DMA_BT_RX, DMA_EV_BLOCK_DONE);
        DCH3INTCLR = _DCH3INT_CHBCIF_MASK;
        DCH3DPTR = 0;
        DCH3SPTR = 0;
        DCH3CPTR = 0;

        //DmaChnSetTxfer(DMA_BT_RX, (void*)&uartReg[UART_BT]->rx.reg, receive.buffer, 1, RN_RECEIVE_BUFFER, 1);

        DCH3CONSET = _DCH3CON_CHEN_MASK;
        //DmaChnEnable(DMA_BT_RX);
    }
}

void __attribute__((vector(DMA_BT_TX_VECTOR), interrupt(ipl3))) ISRWRAPPER_BT_DMA_TX();
void ISR_BT_DMA_TX()
{
	static portBASE_TYPE higherPriorTask;
    higherPriorTask = 0;
    
	// event flags when getting the interrupt
//	INTClearFlag(INT_SOURCE_DMA(DMA_BT_TX));
    IFS1CLR = _IFS1_DMA2IF_MASK;

	//static int evFlags;
	//evFlags = DmaChnGetEvFlags(DMA_BT_TX);	// get the event flags

    //if (evFlags & DMA_EV_BLOCK_DONE)
    if (DCH2INTbits.CHBCIF)
    {
        DCH2INTCLR = _DCH2INT_CHBCIF_MASK;
		//DmaChnClrEvFlags(DMA_BT_TX, DMA_EV_BLOCK_DONE);
        while (U2STAbits.UTXBF);
        //while (!UARTTransmitterIsReady(UART_BT));
        //UARTSendDataByte(UART_BT, PACKET_END);
        U2TXREG = PACKET_END;

        if (autoRelease == 1)
        {
            extern SemaphoreHandle_t semRnTxBus;
            autoRelease = 0;
            xSemaphoreGiveFromISR(semRnTxBus, &higherPriorTask);
        }
        else
        {
            extern SemaphoreHandle_t semRnTxDMA;
            xSemaphoreGiveFromISR(semRnTxDMA, &higherPriorTask);
        }
    }

    portEND_SWITCHING_ISR(higherPriorTask);
}
